package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DriverStationLCD;
public class RobotTemplate extends IterativeRobot {
    DigitalInput LEFT, RIGHT, MID;
    RobotDrive weeeee;
    double DIRECTION; //I'm assuming I can't get my initial direction..
    CANJaguar LeftJag, RightJag;
    double sweep = .3;
    double cycle = 0;
    double maxturntime = 25;
    boolean hasFoundLine = false;
    public void robotInit() {
        DIRECTION = 0;
        while (LeftJag == null)
        {
            try
            {
                LeftJag = new CANJaguar(4);//Front Left
            }catch (Exception e)
            {
                printException("Jag1 failed to initialize",e);
            }
        }
        
        while (RightJag == null)
        {
            try
            {
                RightJag = new CANJaguar(7);//Back Left
            }catch (Exception e)
            {
                printException("Jag2 failed to initialize",e);
            }
        }

        while (LEFT == null)
        {
            try
            {
                LEFT = new DigitalInput(1);
            }catch (Exception e)
            {
                printException("Left Photosensor failed to initilaize",e);
            }
        }
        
        while (MID == null)
        {
            try
            {
                MID = new DigitalInput(2);
            }catch (Exception e)
            {
                printException("Center Photosensor failed to initiliaize",e);
            }
        }
        
        while (RIGHT == null)
        {
            try
            {
                RIGHT = new DigitalInput(3);
            } catch(Exception e)
            {
                printException("Right Photosensor failed to initilaiwerwer",e);
            }
        }

        while (weeeee==null)
        {
            try
            {
                weeeee = new RobotDrive(LeftJag,RightJag);//Not completely sure... Ask someone... MmmmmK?
        }catch(Exception e)
            {
                 printException("Failed to start RobotDrive",e);
            }
        }
    }
    
    public void printException(String error,Exception e)
    {
        System.out.println(error);
        System.out.println(e.toString());
    }
    public void teleopPeriodic() {
        if (!hasFoundLine)
        {
        findLineAndThenDrive(.6);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"Sweeping...");
        }
        else
        {
        alignToLinesAndDriveAndStuff(.4,-.8);
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,"Following...");
        }
        DriverStationLCD.getInstance().updateLCD();
    }
    public void disabledInit()
    {
        //Stop moving
        LeftJag.set(0);
        RightJag.set(0);
    }
    public void findLineAndThenDrive(double speed)
    {
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1,""+cycle);
        if (sweep > 0)
        {
        weeeee.arcadeDrive(speed,.8);
        }
        else
        {
            weeeee.arcadeDrive(speed,-.8);
        }
        cycle+=1;
        if (cycle > maxturntime)
        {
            cycle = 0;
            sweep *= -1;
            if (sweep < 0)
            {
            maxturntime+=10;
            }
        }
        if ((!MID.get())||(!LEFT.get())||(!RIGHT.get()))
        {
            hasFoundLine = true;
            return;
        }
    }
    public void alignToLinesAndDriveAndStuff(double speed,double inc)
    {
        boolean left, right, mid;
        left = !LEFT.get(); mid = !MID.get(); right = !RIGHT.get();
        
        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                DIRECTION+=inc;

               
            }
            if (!left && right)//If we are leaning towards the right...
            {
                DIRECTION-=inc;
                
            }
            if (left && right)//If we are leaning towards the right...
            {
                //I'll need to know our direction...                
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                DIRECTION+=inc;
            
            }
            if (!left && right)//Leaning towards the right a lot?
            {
                DIRECTION-=inc;
              
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
                DIRECTION+=inc;
              
            }
            if (!left && !right)//We are completely lost...
            {
                hasFoundLine = false;
                return;
            }
        }

        weeeee.arcadeDrive(speed, DIRECTION);
        DIRECTION = 0;
    }
}
